/*
 * Wingedbot.c
 *
 * Created: 26-10-2013 13:27:13
 *  Author: Karmu
 */ 

/* define CPU frequency in Mhz here if not defined in Makefile */
#ifndef F_CPU
#define F_CPU 11059200UL			/* 11.0592 MHz crystal clock */ //low fuse EF
#endif

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include "lcd.h"
#include "motor.h"
#include "ps2mouse.h"
#include "servo.h"
#include "uart.h"
#include "uartcmd.h"

#define DELAY		1000

/* defines for LCD (other defines are in LCD.h) */
#define LCDDELAY	50

/* define threshold for ADC */
#define THRESHOLD 800

/* define PWM for Motor Speed control */
/* define TOP & BOTTOM for PWM (ICR1) */
#define TOP				10000 //11.0592 MHz 1 Pre-scalar
#define BOTTOM_R		4300
#define BOTTOM_L		4150

/* define for HC-SR04 Ultrasound Range Sensor */
#define SR_04_TRIGGER	PB3			//PD1 - OUTPUT
#define SR_04_ECHO		PD6			//PD6 - ICP1

/* constant definitions for LCD */
static const PROGMEM unsigned char copyRightChar[] =
{
	0x07, 0x08, 0x13, 0x14, 0x14, 0x13, 0x08, 0x07,
	0x00, 0x10, 0x08, 0x08, 0x08, 0x08, 0x10, 0x00
};
static const PROGMEM unsigned char blockChar[] =
{
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};

/* PID control variables */
#define SETPOINT 15
int prevErrorLeft; //left motor previous error
int prevErrorRight; //right motor previous error
int IerrorLeft=0; //left motor integral error
int IerrorRight=0; //right motor integral error
int encoderCountLeft = 0;
int encoderCountRight = 0;
int setpoint_left = SETPOINT; //setpoint
int setpoint_right = SETPOINT; //setpoint
uint8_t runPIDcount=0; // run PID loop only 10 times a second
volatile uint16_t secondsCount=0; // variable to count seconds in timer0 ISR
volatile int16_t actualPointLeft=0,actualPointRight=0;
//Left PID constants
int   KPl = 15; //PID proportional gain constant
float KIl = 0.1; //PID integral gain constant
float KDl = 220; //PID derivative gain constant
//Right PID constants
int   KPr = 15; //PID proportional gain constant
float KIr = 0.4; //PID integral gain constant
float KDr = 200; //PID derivative gain constant

void init(void);
int16_t get_proximity_sensor_front(void);
int16_t get_proximity_sensor_rear(void);
int16_t get_line_sensor(void);
void booting(void);
void speed_control_left(int newsetpoint);
void speed_control_right(int newsetpoint);
void adc_init(void);
uint16_t adc_read(uint8_t ch);
void pid_timer0_init(void);
void doPID(void);
uint16_t getDistance();

void init(void){
	/* initialize LCD display, cursor off */
	lcd_init(LCD_DISP_ON);
	lcd_clrscr();   /* clear display home cursor */
	/* Booting animation */
	booting();

	/* mouse initialize */
	mouseInit();
	
	/* initialize MOTOR and PWM */
	motor_init();
	motor_pwm_timer1_init(TOP,BOTTOM_R,BOTTOM_L);
	
	/* Initialize timer0 and enable overflow interrupt to read Encoders */
	pid_timer0_init();

	/* Initialize ADC to Read sensor*/
	adc_init();

	/* Initialize Servo and set it to center i.e. 90 degrees*/
	servo_init(8,26);
	servo_set_position(90);
	
	/* enable global interrupts */
	sei();
}

int main(void)
{
	/* Declarations */
	uint16_t line_sensor,proximity_sensor_front,proximity_sensor_rear;
	uint8_t last_direction = GO_FORWARD;    /* last direction steered */
	init();
	
	/*
	while(1)
	{
		steer_robot(GO_FORWARD);
		//setpoint_left=SETPOINT+30;
	}
	*/
	while(1)
	{
		line_sensor=get_line_sensor();
		proximity_sensor_front=get_proximity_sensor_front();
		proximity_sensor_rear=get_proximity_sensor_rear();
		//lcd_putixy(line_sensor,0,0,4);
		//lcd_putixy(proximity_sensor_front,0,1,4);
		//lcd_putixy(proximity_sensor_rear,9,1,4);
		/*
		 0. 0000 - go in circle to find the line turn right

		 1. 0001 - turn left
		 2. 0010 - turn left
		 3. 0011 - turn left
		 7. 0111 - turn left
		10. 1010 - should not occur (go left)
		13. 1101 - should not occur (go left)

		 4. 0100 - turn right
		 5. 0101 - should not occur (go right)
		 8. 1000 - turn right
		11. 1011 - should not occur (go right)
		12. 1100 - turn right
		14. 1110 - turn right

		 6. 0110 - keep moving forward
		 9. 1001 - should not occur (go forward)

		15. 1111 - hard turn left
		
		0 - go in circle to find the line turn right
		1,2,3,7,10,13 - turn left
		4,5,8,11,12,14 - turn right
		6,9 - forward
		15 - hard turn left
		*/
		setpoint_left = SETPOINT; //setpoint
		setpoint_right = SETPOINT; //setpoint
		
		// create the state machine
		switch(line_sensor) {
			case 0:
				steer_robot(last_direction);
				break;
			case 6:
			case 9:
				last_direction=GO_FORWARD;
				steer_robot(last_direction);
				break;
			case 1:
			case 2:
			case 3:
			case 7:
			case 10:
			case 13:
				last_direction=GO_HARD_LEFT;
				steer_robot(last_direction);
				break;
			case 4:
			case 8:
			case 11:
			case 12:
			case 14:
				last_direction=GO_HARD_RIGHT;
				steer_robot(last_direction);
				break;
			case 15:
				last_direction=GO_REVERSE;
				steer_robot(last_direction);
				_delay_ms(90);
				break;
		}
		
	}
}

int16_t get_proximity_sensor_front(void)
{
	uint16_t proximity_result, proximity_result1,proximity_result2;
	proximity_result1 = adc_read(1)>THRESHOLD?1:0;      // read adc value at PA4
	proximity_result2 = adc_read(2)>THRESHOLD?1:0;      // read adc value at PA5
	// (left) proximity_result1 (front) proximity_result2 (right)
	proximity_result=proximity_result2 | proximity_result1<<1;
	
	//lcd_putixy(proximity_result1,0,0,4); //fr
	//lcd_putixy(proximity_result2,0,1,4); //fl
	
	return proximity_result;
}

int16_t get_proximity_sensor_rear(void)
{
	uint16_t proximity_result, proximity_result0, proximity_result3;
	proximity_result0 = adc_read(0)>THRESHOLD?1:0;      // read adc value at PA4
	proximity_result3 = adc_read(3)>THRESHOLD?1:0;      // read adc value at PA7
	// (left) proximity_result0 ( rear) proximity_result3 (right)
	proximity_result=proximity_result0 | proximity_result3<<1;
	
	//lcd_putixy(proximity_result0,9,0,4); //rr
	//lcd_putixy(proximity_result3,9,1,4); //rl
	
	return proximity_result;
}
int16_t get_line_sensor(void)
{
	uint16_t adc_result, adc_result4, adc_result5,adc_result6, adc_result7;
	adc_result4 = adc_read(4)>THRESHOLD?1:0;      // read adc value at PA4
	adc_result5 = adc_read(5)>THRESHOLD?1:0;      // read adc value at PA5
	adc_result6 = adc_read(6)>THRESHOLD?1:0;      // read adc value at PA6
	adc_result7 = adc_read(7)>THRESHOLD?1:0;      // read adc value at PA7
	// (left)adc_result4|adc_result5|adc_result6|adc_result7(right)
	adc_result=adc_result4 | adc_result5<<1 | adc_result6<<2 | adc_result7<<3;
	/*
	lcd_putixy(adc_result7,0,0,4);
	lcd_putixy(adc_result6,0,1,4);
	lcd_putixy(adc_result5,9,0,4);
	lcd_putixy(adc_result4,9,1,4);
	*/
	return adc_result;
}
void booting(void)
{
	int i;
	lcd_puts("-Line Follower-\n");
	/*
    * load two user defined characters from program memory
    * into LCD controller CG RAM location 0 and 1
    */
	lcd_command(_BV(LCD_CGRAM));  /* set CG RAM start address 0 */
	for(i=0; i<16; i++)
	{
		lcd_data(pgm_read_byte_near(&blockChar[i]));
	}
	for(i=0; i<16; i++)
    {
		/* create illusion of progress bar */
		_delay_ms(LCDDELAY);
		/* increment cursor to position i on line 2 */
        lcd_gotoxy(i,1);  
        /* write single char to display */
        lcd_putc(0);
	}	
	
	lcd_clrscr();
	lcd_puts("Avoibot Running\n");
}

void speed_control_left(int newsetpoint){

	setpoint_left = newsetpoint;
}
void speed_control_right(int newsetpoint){

	setpoint_right = newsetpoint;
}

void adc_init(void)
{	
	// initialize adc
	// AREF = AVcc
	ADMUX = (1<<REFS0);
	
	// ADC Enable and prescaler of 128
	// 11059200/128 = 86400
	ADCSRA = (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
}

uint16_t adc_read(uint8_t ch)
{	
	// read adc value
	// select the corresponding channel 0~7
	// ANDing with '7' will always keep the value
	// of 'ch' between 0 and 7
	ch &= 0b00000111;  // AND operation with 7
	ADMUX = (ADMUX & 0xF8)|ch;     // clears the bottom 3 bits before ORing
	
	// start single conversion
	// write '1' to ADSC
	ADCSRA |= (1<<ADSC);
	
	// wait for conversion to complete
	// ADSC becomes '0' again
	// till then, run loop continuously
	while(ADCSRA & (1<<ADSC));
	
	return (ADC);
}

void pid_timer0_init(void)
{
	TCNT0=0x00;
	TCCR0A |= (1 << WGM01); // Configure timer 0 for CTC mode
	TCCR0B |= (1<<CS02) | (1<<CS00); //Pre-scaler 1024, (42.353 interrupts/s in Normal Mode)
	//TIMSK|=(1<<TOIE0); //Enable timer0 Overflow interrupts
	TIMSK0 |= (1 << OCIE0A ); // Enable CTC interrupt
	
	OCR0A=0x96; //72 clicks per second
}

ISR(TIMER0_COMPA_vect)
{
	//Timer ISR on compare match to run PID
	runPIDcount+=1;	
	if (runPIDcount>=10){
		runPIDcount=0;
		//used for Quadrature Encoder - we will instead use MOUSE to get decoded quadrature code.
		//reading encoders on left and right motor
		scanMouse();
		actualPointLeft=mouseNewY;
		actualPointRight=mouseNewX;
		//lcd_putixy(actualPointLeft,0,1,4);
		//lcd_putixy(actualPointRight,9,1,4);
		if(pidSwitchRight==1 || pidSwitchLeft==1) doPID();
		secondsCount+=1;
	}
}

void doPID(void)
{
	int errorLeft; //error from left encoder
	int errorRight; //error from right encoder
	int dErrorLeft; //derivative error left
	int dErrorRight; //derivative error right
	int cLeft;
	int cRight;

	encoderCountLeft=actualPointLeft;//mouseNewY;
	encoderCountRight=actualPointRight;//mouseNewX;

	errorLeft = setpoint_left - encoderCountLeft; //calculate error values
	errorRight = setpoint_right - encoderCountRight;

	//encoderCountLeft = 0; //reset encoder counts ready for next sample
	//encoderCountRight = 0;

	dErrorLeft = (errorLeft - prevErrorLeft);
	dErrorRight = (errorRight - prevErrorRight);

	cLeft = ((KPl*errorLeft) + (KDl*dErrorLeft) + (KIl*IerrorLeft)); //PID equations
	cRight = ((KPr*errorRight) + (KDr*dErrorRight) + (KIr*IerrorRight));

	if(pidSwitchLeft==1) {
		if((OCR1A + cLeft) > TOP) //prevent OCR0 and OCR2 from overrunning 255
		OCR1A = TOP;
		else
		OCR1A = OCR1A + cLeft; //use output from PID equations to alter motor speeds
	}
	if(pidSwitchRight==1) {
		if((OCR1B + cRight) > TOP) //where OCR0 and OCR2 are PWM output values
		OCR1B = TOP;
		else
		OCR1B = OCR1B + cRight;
	}
	
	prevErrorLeft = errorLeft; //set previous error to current error
	prevErrorRight = errorRight;

	IerrorLeft = IerrorLeft + errorLeft; //add current error to integral error
	IerrorRight = IerrorRight + errorRight;
}

uint16_t getDistance()
{
	uint16_t t1 = 0, t2 = 0;
	// Simple steps for generating a trigger pulse at trigger pin
	PORTB|=1<<SR_04_TRIGGER;
	_delay_us(15);
	PORTB&=~(1<<SR_04_TRIGGER);
	
	TCCR1A = 0;
	TCCR1B |= (1<<CS11) | (1<<CS10);   // Timer operating freq is 11.0592MHz/64 (from pre-scalar) gives min. resolution of 5.79us
	
	// ICES1 = 1 for rising edge detection on ICP1
	TCCR1B |= (1<<ICES1);
	
	while((TIFR1&(1<<ICF1))==0);   // polled until rising edge detected
	t1 = ICR1;               // Storing the timer value
	
	TIFR1 = 1<<ICF1;         // Clearing the flag for next use
	
	// ICES1 = 1 for rising edge detection on ICP1
	TCCR1B &=~_BV(ICES1);
	while((TIFR1&(1<<ICF1))==0);   // polled until falling edge detected
	t2 = ICR1;               // Storing the timer value
	
	TIFR1 = 1<<ICF1;         // Clearing the flag for next use
	
	TCCR1B = 0;               //stop timer
	
	return ((t2-t1)*5.79)/58;
}